#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
 
int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("/root/PCL/Chinadragon.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file tabletop.pcd \n");
        return (-1);
    }
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setSearchMethod(tree);

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(0.03);
    ne.compute(*cloud_normals);
    // 8. 创建一个带颜色信息的点云对象，用于存储根据曲率设置颜色后的点云数据
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    colored_cloud->width = cloud->width;
    colored_cloud->height = cloud->height;
    colored_cloud->points.resize(cloud->points.size());
 
    // 9. 遍历每个点，将曲率值映射为颜色，并赋值给colored_cloud
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        // 将原始点云中的坐标信息复制到有颜色的点云对象中
        colored_cloud->points[i].x = cloud->points[i].x;
        colored_cloud->points[i].y = cloud->points[i].y;
        colored_cloud->points[i].z = cloud->points[i].z;
 
        // 获取当前点的曲率值
        float curvature = cloud_normals->points[i].curvature;
        
        // 根据曲率值设置颜色，曲率大的点用红色表示，曲率小的点用蓝色表示
        uint8_t r = static_cast<uint8_t>(255 * curvature); // 红色分量
        uint8_t g = 0;                                     // 绿色分量
        uint8_t b = static_cast<uint8_t>(255 * (1.0 - curvature)); // 蓝色分量
 
        // 将颜色值组合成一个32位整数，并赋给点的rgb字段
        uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
                        static_cast<uint32_t>(g) << 8 | 
                        static_cast<uint32_t>(b));
        
        colored_cloud->points[i].rgb = *reinterpret_cast<float*>(&rgb);
    }
 
    // 10. 创建一个PCLVisualizer对象，用于可视化点云数据
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Curvature Viewer"));
    
    // 11. 设置可视化窗口的背景色为黑色
    viewer->setBackgroundColor(0, 0, 0);
    
    // 12. 将带颜色信息的点云添加到可视化窗口中，并设置显示属性
    viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "colored cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "colored cloud");
 
    // 13. 进入可视化循环，直到用户关闭窗口
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }
 
    return 0;


}